#ifndef FLIGHT_CONTROL_CONFIG_H
#define FLIGHT_CONTROL_CONFIG_H

#include <string.h>
using namespace std;
#include <unistd.h>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Twist.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <nav_msgs/Odometry.h>

#include "px4_flight/show_status.h"
#include "px4_flight/flight_status_control.h"
#include "px4_flight/flight_target_control.h"


// 参数设置
static double hover_alt = 2.0;          // 悬停高度(m)
static double hover_duration = 10.0;    // 悬停时间(s)
static double vistik_timeout = 60.0;    // 虚拟摇杆超时时间(s)
static double arm_timeout = 10.0;       // 解锁超时(s)
static double takeoff_timeout = 30.0;   // 起飞超时(s)
static double half_alt = (hover_alt + 0.4) / 2;  //降落中途点


// 飞行状态枚举
enum FlightPhase {
    WAITING = 0,        // 等待     0
    INIT    = 1,        // 初始化   1
    ARMING  = 2,        // 解锁中   2
    TAKEOFF = 3,        // 起飞中   3
    HOVER   = 4,        // 悬停中   4  
    LAND    = 5,        // 降落后   5
    DISARM  = 6,        // 上锁     6
    VSTICK  = 8,        // 虚拟摇杆  8

    ARM_FIN = 7         // 解锁完成  7
};

extern FlightPhase phase;
FlightPhase int_to_enum(const int request);
string enum_to_str(const FlightPhase phase);

extern mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg);

extern nav_msgs::Odometry current_local_statu;
extern geometry_msgs::Point current_local_pose;
void get_local_pose(const nav_msgs::Odometry::ConstPtr& msg);

bool flight_cb(px4_flight::flight_status_control::Request &request, 
        px4_flight::flight_status_control::Response &response);

extern mavros_msgs::PositionTarget target_pose;
extern ros::Time last_target_request;
bool target_cb(px4_flight::flight_target_control::Request &request, 
        px4_flight::flight_target_control::Response &response);

void px4_flight_control();




// 位掩码组合定义
namespace ControlMasks {
    // 基本忽略位
    const uint16_t IGNORE_PX   = mavros_msgs::PositionTarget::IGNORE_PX;
    const uint16_t IGNORE_PY   = mavros_msgs::PositionTarget::IGNORE_PY;
    const uint16_t IGNORE_PZ   = mavros_msgs::PositionTarget::IGNORE_PZ;
    const uint16_t IGNORE_VX   = mavros_msgs::PositionTarget::IGNORE_VX;
    const uint16_t IGNORE_VY   = mavros_msgs::PositionTarget::IGNORE_VY;
    const uint16_t IGNORE_VZ   = mavros_msgs::PositionTarget::IGNORE_VZ;
    const uint16_t IGNORE_AFX  = mavros_msgs::PositionTarget::IGNORE_AFX;
    const uint16_t IGNORE_AFY  = mavros_msgs::PositionTarget::IGNORE_AFY;
    const uint16_t IGNORE_AFZ  = mavros_msgs::PositionTarget::IGNORE_AFZ;
    const uint16_t IGNORE_YAW  = mavros_msgs::PositionTarget::IGNORE_YAW;
    const uint16_t IGNORE_YAW_RATE = mavros_msgs::PositionTarget::IGNORE_YAW_RATE;

    // 常用组合模式
    const uint16_t VELOCITY_CONTROL = 
        IGNORE_PX | IGNORE_PY | IGNORE_PZ |
        IGNORE_AFX | IGNORE_AFY | IGNORE_AFZ |
        IGNORE_YAW | IGNORE_YAW_RATE;

    const uint16_t POSITION_CONTROL = 
        IGNORE_VX | IGNORE_VY | IGNORE_VZ |
        IGNORE_AFX | IGNORE_AFY | IGNORE_AFZ |
        IGNORE_YAW | IGNORE_YAW_RATE;

    const uint16_t POSITION_WITH_YAW_CONTROL = 
        IGNORE_VX | IGNORE_VY | IGNORE_VZ |
        IGNORE_AFX | IGNORE_AFY | IGNORE_AFZ |
        IGNORE_YAW_RATE;

    const uint16_t VELOCITY_WITH_YAW_CONTROL = 
        IGNORE_PX | IGNORE_PY | IGNORE_PZ |
        IGNORE_AFX | IGNORE_AFY | IGNORE_AFZ |
        IGNORE_YAW_RATE;

    const uint16_t VELOCITY_WITH_YAW_RATE_CONTROL = 
        IGNORE_PX | IGNORE_PY | IGNORE_PZ |
        IGNORE_AFX | IGNORE_AFY | IGNORE_AFZ |
        IGNORE_YAW;

    const uint16_t ACCELERATION_CONTROL = 
        IGNORE_PX | IGNORE_PY | IGNORE_PZ |
        IGNORE_VX | IGNORE_VY | IGNORE_VZ |
        IGNORE_YAW | IGNORE_YAW_RATE;
}



#endif